//
// Created by host on 8/10/25.
//

#ifndef ROBOT_PLAN_H
#define ROBOT_PLAN_H

#include <memory>
#include <vector>

#include "data_structure.h"
#include "model/differential_kinematics.hpp"
#include "model/inverse_kinematics.hpp"
#include "plan/planner_time_optimal_georgia.hpp"

namespace ZORAL
{
    class RobotPlan
    {
    private:
        double duration_;
        PlanMethod plan_method_;
        PlannerType planner_type_;
        Eigen::MatrixXd param_five_poly_;
        std::vector<HomoTrans> interp_pose_;
        std::shared_ptr<PlannerTimeOptimalGeorgia> param_georgia_;
        std::shared_ptr<ForwardKinematics> fk_;
        std::shared_ptr<DifferentialKinematics> dk_;
        std::shared_ptr<InverseKinematics> ik_;
        Eigen::VectorXd position_last_;
        int dof_;

    public:
        RobotPlan(std::shared_ptr<DifferentialKinematics> dk, std::shared_ptr<InverseKinematics> ik) :
            duration_(0), plan_method_(), planner_type_()
        {
            dk_ = std::move(dk);
            ik_ = std::move(ik);
            dof_ = ik_->getRobotDof();
            fk_ = ik_->getForwardKinematics();
        }
        ~RobotPlan() = default;

        //! TODO: 用path来定义
        int moveJoint(const std::vector<VectorXd> &position, const std::vector<VectorXd> &velocity,
                      const std::vector<VectorXd> &acceleration, const PlannerProperties &properties);

        int movePose(const VectorXd &start_joint_position, const VectorXd &start_joint_velocity,
                     const VectorXd &start_joint_acceleration, const std::vector<VectorXd> &cartesian_positions,
                     const std::vector<VectorXd> &cartesian_velocities,
                     const std::vector<VectorXd> &cartesian_accelerations, const PlannerProperties &properties);

        int moveLine(const VectorXd &start_joint_position, const VectorXd &start_joint_velocity,
                     const VectorXd &start_joint_acceleration, const std::vector<VectorXd> &cartesian_positions,
                     const std::vector<VectorXd> &cartesian_velocities,
                     const std::vector<VectorXd> &cartesian_accelerations, const PlannerProperties &properties);

        int moveNullspace(const VectorXd &start_joint_position, const VectorXd &start_joint_velocity,
                          const VectorXd &start_joint_acceleration, const std::vector<double> &nullspace_positions,
                          const std::vector<double> &nullspace_velocities,
                          const std::vector<double> &nullspace_accelerations, const PlannerProperties &properties);

        int getJointPoint(const double &t, VectorXd &position, VectorXd &velocity, VectorXd &acceleration);

        double getDuration() const;
    };
} // namespace ZORAL


#endif // ROBOT_PLAN_H
